#include <ros/ros.h>
#include "serial/serial.h"
#include <geometry_msgs/Twist.h>
#include <nav_msgs/Odometry.h>
#include <nav_msgs/Path.h>   //发布轨迹用
#include <std_msgs/String.h>

#include <tf2/utils.h>
//#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/transform_broadcaster.h>

//#include <tf2/LinearMath/Quaternion.h>
#include <chrono>  

#include <thread>  
struct MyMessage
{
    // CAN 模块 切换  AT模式
    std::string command = "AT+AT\r\n";
    // 操作模式
    std::vector<uint8_t> data0 = {0x41, 0x54, 0x00, 0x00, 0x00, 0x00, 0x02, 0x01, 0x00, 0x0D, 0x0A};

    // 3号速度模式
    std::vector<uint8_t> data1_1 = {0x41, 0x54, 0xC0, 0x60, 0x00, 0x00, 0x08, 0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00, 0x0D, 0x0A};
    // 3号使能
    std::vector<uint8_t> data1_2 = {0x41, 0x54, 0xC0, 0x60, 0x00, 0x00, 0x08, 0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x0D, 0x0A};

    // 4号速度模式
    std::vector<uint8_t> data2_1 = {0x41, 0x54, 0xC0, 0x80, 0x00, 0x00, 0x08, 0x2F, 0x60, 0x60, 0x00, 0x03, 0x00, 0x00, 0x00, 0x0D, 0x0A};
    // 4号使能
    std::vector<uint8_t> data2_2 = {0x41, 0x54, 0xC0, 0x80, 0x00, 0x00, 0x08, 0x2B, 0x40, 0x60, 0x00, 0x0F, 0x00, 0x00, 0x00, 0x0D, 0x0A};

    // 3号右轮
    std::vector<uint8_t> data1_3 = {0x41, 0x54, 0xC0, 0x60, 0x00, 0x00, 0x08, 0x23, 0xFF, 0x60, 0x00};
    // 4号左轮
    std::vector<uint8_t> data2_3 = {0x41, 0x54, 0xC0, 0x80, 0x00, 0x00, 0x08, 0x23, 0xFF, 0x60, 0x00};

    // 3号轮速度读取
    std::vector<uint8_t> data1_4 = {0x41, 0x54, 0xC0, 0x60, 0x00, 0x00, 0x08, 0x40, 0x69, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D, 0x0A};
    // 4号轮速度读取
    std::vector<uint8_t> data2_4 = {0x41, 0x54, 0xC0, 0x80, 0x00, 0x00, 0x08, 0x40, 0x69, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D, 0x0A};

    std::vector<uint8_t> data_vel_read_conbin = {0x41, 0x54, 0xC0, 0x60, 0x00, 0x00, 0x08, 0x40, 0x69, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D, 0x0A, 0x41, 0x54, 0xC0, 0x80, 0x00, 0x00, 0x08, 0x40, 0x69, 0x60, 0x00, 0x00, 0x00, 0x00, 0x00, 0x0D, 0x0A};
};

class Veh_Node
{
public:
    // 构造函数，接受节点名称作为参数
    Veh_Node(ros::NodeHandle &nh) : nh_(nh) // 使用节点名称初始化NodeHandle
    {
        
        setlocale(LC_CTYPE, "zh_CN.utf8");
        // 串口初始化
        InitVehSerial();
        // 电机初始化
        VehEnInit();
        _time_now = ros::Time::now();
        _time_last = ros::Time::now();

        // odom_piblisher = nh_.advertise<nav_msgs::Odometry>("veh_odom", 50);
        // path_piblisher = nh_.advertise<nav_msgs::Path>("veh_path", 1);

        tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>();

        timer_VelRead = nh_.createTimer(ros::Duration(0.05), &Veh_Node::VelRead_callback, this); // 速度读取 定时器

        // timer_VelSend = nh_.createTimer(ros::Duration(0.1), &Veh_Node::VelSend_callback, this);  // 速度发送 定时器

        // timer_Odom = nh_.createTimer(ros::Duration(0.1), &Veh_Node::Odom_callback, this);  // Odom发布

        cmd_vel_sub = nh_.subscribe("cmd_vel", 1000, &Veh_Node::Vel_cmd_callback, this); // cmd_vel速度话题订阅
    }

    void InitVehSerial()
    {
        try
        {   
            ser.setPort("/dev/ttyUSB_veh_CAN");
            ser.setBaudrate(115200);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000);
            ser.setTimeout(to);
            ser.open();
        }
        catch (serial::IOException &e)
        {
            ROS_ERROR_STREAM("Unable to open serial");
            return;
        }
        if (ser.isOpen())
        {
            ROS_INFO("Veh_COM is OPEN!");
        }
    };

    void VehEnInit()
    {
        if (ser.isOpen())
        {
            ser.write(cmd_msg.command.c_str());
            std::this_thread::sleep_for(std::chrono::milliseconds(100));

            ser.write(cmd_msg.data0.data(), cmd_msg.data0.size());

            std::this_thread::sleep_for(std::chrono::milliseconds(100));

            // 3号 左轮电机初始
            ser.write(cmd_msg.data1_1.data(), cmd_msg.data1_1.size());
            ROS_INFO("电机3速度模式指令发送");
            std::this_thread::sleep_for(std::chrono::milliseconds(100));

            ser.write(cmd_msg.data1_2.data(), cmd_msg.data1_2.size());
            ROS_INFO("电机3使能指令发送");
            std::this_thread::sleep_for(std::chrono::milliseconds(100));

            // 4号 右轮电机初始
            ser.write(cmd_msg.data2_1.data(), cmd_msg.data2_1.size());
            ROS_INFO("电机4速度模式指令发送");
            std::this_thread::sleep_for(std::chrono::milliseconds(100));

            ser.write(cmd_msg.data2_2.data(), cmd_msg.data2_2.size());
            ROS_INFO("电机4使能指令发送");

            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            ser.flush();
        }
    };

    // vel_cmd 话题 回调函数
    void Vel_cmd_callback(const geometry_msgs::Twist cmd_vel)
    {
        cmd_x = cmd_vel.linear.x;
        cmd_y = cmd_vel.linear.y;
        cmd_z = cmd_vel.linear.z;

        cmd_ang_x = cmd_vel.angular.x;
        cmd_ang_y = cmd_vel.angular.y;
        cmd_ang_z = cmd_vel.angular.z;
        //ROS_INFO("Linear Components:[%f,%f,%f]", cmd_vel.linear.x, cmd_vel.linear.y, cmd_vel.linear.z);

        // 直接发送速度
        double Vr = cmd_x + cmd_ang_z * wheel_separation_ / 2;
        double Vl = cmd_x - cmd_ang_z * wheel_separation_ / 2;

        // 3号左轮
        double vel_3l_1 = 10 * Vl / (2 * 3.141 * wheelR);
        int vel_3l = vel_3l_1 * 16384;

        std::vector<uint8_t> vel_data_3 = cmd_msg.data1_3;
        // 将 vel_data_3 转换成中间四个16进制数据位
        for (int i = 0; i < 4; ++i)
        {
            vel_data_3.push_back((uint8_t)(vel_3l >> (8 * i)) & 0xFF);
        }

        vel_data_3.push_back(0x0D); // 回车符
        vel_data_3.push_back(0x0A); // 换行符

        // 4号右轮
        double vel_4r_1 = -10 * Vr / (2 * 3.141 * wheelR);
        int vel_4r = vel_4r_1 * 16384;

        std::vector<uint8_t> vel_data_4 = cmd_msg.data2_3;
        for (int i = 0; i < 4; ++i)
        {
            vel_data_4.push_back((uint8_t)(vel_4r >> (8 * i)) & 0xFF);
        }

        vel_data_4.push_back(0x0D); // 回车符
        vel_data_4.push_back(0x0A); // 换行符

        // RCLCPP_INFO(this->get_logger(), "3号发送报文%s",vectorToHex(vel_data_3).c_str());

        std::vector<uint8_t> combined_data;

        combined_data.insert(combined_data.end(), vel_data_3.begin(), vel_data_3.end());
        combined_data.insert(combined_data.end(), vel_data_4.begin(), vel_data_4.end());

        ser.write(combined_data.data(), combined_data.size());
    }


    void VelSend_callback(const ros::TimerEvent &event)
    {
        double Vr = cmd_x + cmd_ang_z * wheel_separation_ / 2;
        double Vl = cmd_x - cmd_ang_z * wheel_separation_ / 2;

        // 3号左轮
        double vel_3l_1 = 10 * Vl / (2 * 3.141 * wheelR);
        int vel_3l = vel_3l_1 * 16384;

        std::vector<uint8_t> vel_data_3 = cmd_msg.data1_3;
        // 将 vel_data_3 转换成中间四个16进制数据位
        for (int i = 0; i < 4; ++i)
        {
            vel_data_3.push_back((uint8_t)(vel_3l >> (8 * i)) & 0xFF);
        }
        // uint8_t byte3_1 = vel_3l & 0xFF;        // 获取低字节
        // uint8_t byte3_2 = (vel_3l >> 8) & 0xFF; // 获取高字节（如果有的话）
        // uint8_t byte3_3 = (vel_3l >> 16) & 0xFF;
        // uint8_t byte3_4 = (vel_3l >> 24) & 0xFF;

        // vel_data_3.push_back(byte3_1); // 先追加低字节
        // vel_data_3.push_back(byte3_2); // 再追加高字节
        // vel_data_3.push_back(byte3_3); // 再追加高字节
        // vel_data_3.push_back(byte3_4); // 再追加高字节
        vel_data_3.push_back(0x0D); // 回车符
        vel_data_3.push_back(0x0A); // 换行符

        // 4号右轮
        double vel_4r_1 = -10 * Vr / (2 * 3.141 * wheelR);
        int vel_4r = vel_4r_1 * 16384;

        std::vector<uint8_t> vel_data_4 = cmd_msg.data2_3;
        for (int i = 0; i < 4; ++i)
        {
            vel_data_4.push_back((uint8_t)(vel_4r >> (8 * i)) & 0xFF);
        }

        vel_data_4.push_back(0x0D); // 回车符
        vel_data_4.push_back(0x0A); // 换行符

        // RCLCPP_INFO(this->get_logger(), "3号发送报文%s",vectorToHex(vel_data_3).c_str());

        std::vector<uint8_t> combined_data;

        combined_data.insert(combined_data.end(), vel_data_3.begin(), vel_data_3.end());
        combined_data.insert(combined_data.end(), vel_data_4.begin(), vel_data_4.end());

        ser.write(combined_data.data(), combined_data.size());
    }

    void VelRead_callback(const ros::TimerEvent &event)
    {
        if (ser.isOpen() != 0)
        {
            ser.write(cmd_msg.data_vel_read_conbin.data(), cmd_msg.data_vel_read_conbin.size()); // 读速度指令：3，4电机

            while (ser.available() >= 2)
            {
                std::string data_byte = ser.readline();
                if (data_byte.size() > 15 && static_cast<uint8_t>(data_byte[0]) == 0x41 && static_cast<uint8_t>(data_byte[3]) == 0x60 && static_cast<uint8_t>(data_byte[8]) == 0x69)
                {
                    int32_t value3 = 0;
                    for (int i = 14; i > 10; i--)
                    {
                        value3 <<= 8;
                        value3 |= (data_byte[i] & 0xff);
                        std::cout << std::hex << (data_byte[i] & 0xff) << " ";
                    }
                    vel_3 = static_cast<double>(value3) / 16384 / 10 * 6.28 * wheelR;
                    // std::cout << "3号Combined int: " << vel_3 << std::endl;
                    // RCLCPP_INFO(this->get_logger(), "3号报文%f",vel_3);
                    std::cout << std::fixed << std::setprecision(2) << "3号线速度: " << vel_3 << " m/s" << std::endl;
                }
                else if (data_byte.size() > 15 && static_cast<uint8_t>(data_byte[0]) == 0x41 && static_cast<uint8_t>(data_byte[3]) == 0x80 && static_cast<uint8_t>(data_byte[8]) == 0x69)
                {
                    int32_t value4 = 0;
                    for (int i = 14; i > 10; i--)
                    {
                        value4 <<= 8;
                        value4 |= (data_byte[i] & 0xff);
                        // value |= static_cast<uint32_t>(data_byte[i]);
                        //  std::cout << std::hex << (data_byte[i] & 0xff) << " ";
                    }
                    // 4号<右轮>反转为前进，加负号
                    vel_4 = -(static_cast<double>(value4) / 16384 / 10 * 6.28 * wheelR);
                    // RCLCPP_INFO(this->get_logger(), "4号报文%f",vel_4);
                    std::cout << std::fixed << std::setprecision(2) << "4号线速度: " << vel_4 << " m/s" << std::endl;
                }
                data_byte.clear();
            }
        }
    }

    void Odom_callback(const ros::TimerEvent &event)
    {
    //  <<<<<<<<<<<<<<     Odom发布    >>>>>>>>>>>>>>>>>
    _time_now = ros::Time::now();
    double sampling_time = (_time_now - _time_last).toSec();
            //速度和角速度
    double V = (vel_3 + vel_4)/2;
    double W = (vel_4-vel_3)/wheel_separation_;

    Robot_Pos.X += sampling_time * V * cos(Robot_Angle.Z  );
    Robot_Pos.Y += sampling_time * V * sin(Robot_Angle.Z );
    Robot_Pos.Z = 0;
    Robot_Angle.Z += sampling_time*W; //航向

    tf2::Quaternion quaternion;
    quaternion.setRPY(0.0, 0.0, Robot_Angle.Z);
    //geometry_msgs::msg::Quaternion odom_quat = tf2::toMsg(quaternion);
    geometry_msgs::Quaternion odom_quat = tf2::toMsg(quaternion);
    odom.header.stamp = _time_now;
    odom.header.frame_id = "odom";

    //机器人在轴上的平移量
    odom.pose.pose.position.x = Robot_Pos.X;
    odom.pose.pose.position.y = Robot_Pos.Y;
    odom.pose.pose.position.z = 0.0;

    odom.pose.pose.orientation = odom_quat;
    //旋转信息
    odom.child_frame_id = "base_link";
    Robot_Vel.X = V;
    Robot_Vel.Y = 0;
    Robot_Vel.Z = 0;
    odom.twist.twist.linear.x = Robot_Vel.X;  // X方向前进速度
    odom.twist.twist.linear.y = Robot_Vel.Y;  // y方向前进速度
    odom.twist.twist.angular.z = W; // 角速度

    odom_piblisher.publish(odom);
    //  <<<<<<<<<<<<<<     Odom  tf2变换发布    >>>>>>>>>>>>>>>>>
    transform.header.frame_id = "odom";
    transform.child_frame_id = "base_footprint";
    transform.header.stamp = _time_now;

    transform.transform.translation.x = odom.pose.pose.position.x;
    transform.transform.translation.y = odom.pose.pose.position.y;
    transform.transform.translation.z = odom.pose.pose.position.z;
    transform.transform.rotation.x = odom.pose.pose.orientation.x;
    transform.transform.rotation.y = odom.pose.pose.orientation.y;
    transform.transform.rotation.z = odom.pose.pose.orientation.z;
    transform.transform.rotation.w = odom.pose.pose.orientation.w;
    tf_broadcaster_->sendTransform(transform);

    // <<<<<<<<<<<<<<     Path 发布    >>>>>>>>>>>>>>>>>
    geometry_msgs::PoseStamped pose_stamped;

    pose_stamped.header.stamp = _time_now;
    pose_stamped.header.frame_id = "odom";
    pose_stamped.pose.position.x = Robot_Pos.X;
    pose_stamped.pose.position.y = Robot_Pos.Y;
    pose_stamped.pose.position.z = 0.0;
    pose_stamped.pose.orientation = odom_quat;


    path.header.stamp = _time_now;
    path.poses.push_back(pose_stamped);
    path.header.frame_id = "odom";
    path_piblisher.publish(path);

    _time_last = _time_now; // 记录时间
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber cmd_vel_sub;
    ros::Publisher odom_piblisher;
    ros::Publisher path_piblisher;
    //std::string message;

    nav_msgs::Odometry odom;
    nav_msgs::Path path;
    geometry_msgs::TransformStamped transform;
    std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

    ros::Timer timer_VelRead; // 速度读取
    ros::Timer timer_VelSend; // 速度读取
    ros::Timer timer_Odom;

    serial::Serial ser;
    MyMessage cmd_msg;  // 电机报文

    double wheelR = 0.192;
    double wheel_separation_ = 0.5; //轮距
    double vel_3,vel_4;   // 读到的车速

    float cmd_x = 0;
    float cmd_y = 0;
    float cmd_z = 0;
    float cmd_ang_x = 0;
    float cmd_ang_y = 0;
    float cmd_ang_z = 0;

    typedef struct __Vel_Pos_Data_
    {
        double X;
        double Y;
        double Z;
    } Vel_Pos_Data;
    Vel_Pos_Data Robot_Pos; // 机器人的位置
    Vel_Pos_Data Robot_Angle;
    Vel_Pos_Data Robot_Vel; // 机器人的速度
    //double sampling_time;   // 采样时间
    ros::Time _time_now, _time_last;
};

int main(int argc, char **argv)
{
    // 初始化ROS系统，并指定节点名称（这里也可以不指定，但在构造函数中需要显式传递）
    ros::init(argc, argv, "veh_ctr_node");
    ros::NodeHandle nh; // NodeHandle对象，用于与ROS系统交互

    Veh_Node Veh_node(nh);

    ros::spin(); // 处理回调函数队列中的一次回调

    return 0;
}
